#! /usr/bin/env python
import rospy
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import *
import time
import numpy as np
if __name__ =="__main__":
    rospy.init_node('p_robot_env')
    rospy.wait_for_service('/gazebo/set_model_state')
        
    set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
    set_board = SetModelStateRequest()  # Create an object of type SetModelStateRequest
        
    # set red cube pose
    set_board.model_state.model_name = "checkerboard_7_7_0_05_0"
    set_board.model_state.pose.position.x = 0.5
    set_board.model_state.pose.position.y = 0.5
    set_board.model_state.pose.position.z = 0.5
    set_board.model_state.pose.orientation.w = 1
    set_board.model_state.pose.orientation.x = 0
    set_board.model_state.pose.orientation.y = 0
    set_board.model_state.pose.orientation.z = 0

    set_board.model_state.twist.linear.x = 0.0
    set_board.model_state.twist.linear.y = 0.0
    set_board.model_state.twist.linear.z = 0.0
    set_board.model_state.twist.angular.x = 0.0
    set_board.model_state.twist.angular.y = 0.0
    set_board.model_state.twist.angular.z = 0.0
    set_board.model_state.reference_frame = "world"
    x_bias = -0.0;
    y_bias = -0.6;
    z_bias = 0.0;
    dx = 0.5;
    dy = 0.5;
    dz = 0.5;
    while(True):
        qx = 0.2*(( (np.random.randint(1,32767+1)%100)/100.0)-0.5)
        qy = 0.2*(( (np.random.randint(1,32767+1)%100)/100.0)-0.5)
        qz = 0.2*(( (np.random.randint(1,32767+1)%100)/100.0)-0.5)
        qw =  0.5
        norm = np.sqrt([qx,qy,qz,qw])
        set_board.model_state.pose.orientation.w = norm[-1]
        set_board.model_state.pose.orientation.x = norm[0]
        set_board.model_state.pose.orientation.y = norm[1]
        set_board.model_state.pose.orientation.z = norm[2]

        x = x_bias + dx*((np.random.randint(1,32767+1)%100)/100.0-0.5); 
        y = y_bias + dy*((np.random.randint(1,32767+1)%100)/100.0-0.5); 
        z = z_bias + dz*((np.random.randint(1,32767+1)%100)/100.0-0.5); 
        set_board.model_state.pose.position.x = x
        set_board.model_state.pose.position.y = y
        set_board.model_state.pose.position.z = z
        time.sleep(0.5)
        set_state_service(set_board)